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1, Introduction 

3D imaging systems are line-of-sight instruments 
that provide range images of objects in a given region 
of interest, I('&,(p), where / denotes the distance from an 
instrument to a point on a surface of the object and '& 
and (p are the elevation and the azimuth angles to that 
point. Usually a point cloud in a Cartesian coordinate 
system associated with the instrument is derived from 
the range image. Current 3D imaging systems may 
collect point clouds containing hundreds of thousands 
of points within a few seconds [1]. Frequently, these 
data points are used to model the objects by geo- 
mefrical primitives that are characterized by atfributes 
such as location, pose, width, height, etc. Numerical 
values of these attributes may be obtained by fitting a 
model to the segmented dataset. Here, we discuss the 
Nonlinear Least Squares (NLS) fitting procedure 
applied to range data obtained by scanning a plane. 
Specifically, we address the following problem: how 
do the uncertainties of range measurements by an 
instrument propagate to uncertainties of the fitted plane 
parameters [2]? 



Usually, variances of fitted parameters (which are 
useful for uncertainty analysis) are derived from the 
Jacobian mafrix of a model function used in a given 
fitting problem. This common approach is based on a 
linearization of the nonlinear error function near its 
minimum [3-7]. In this paper we do not follow this path 
but estimate the variances directly from the sensitivities 
for which we provide analytical formulas. 



2, Variances of Fitted Plane Parameters 

A plane in a three-dimensional Cartesian coordinate 
system is defined as a set of points P{x, y, z) satisfying 
the following equation 



P{x,y,z)»w{T»,(p) = D 



(1) 



where w{^,(p) is a unit vector perpendicular to a plane, 
parameterized by two angles: the elevation •& (a zero 
elevation being horizontal) and the azimuth (p, and 
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where • stands for the dot product of two vectors. The 
Cartesian coordinates of w(!?,(p) may be written as 



w{i}, (p) = [cos i?cos (p, COS 7?sin cp, sin i?] . 



(2) 



The absolute value of parameter D is the distance 
from the plane to the origin of the coordinate system, 
and D may be expressed as 



D = Po{Xo,y„,z„)»wiT},(p), 



(3) 



where Pq is any point on a plane. A plane can be fit to 
the experimental dataset P^p,., = {PjJ = 1 , . . . , A^} , where 
A^ denotes the number of points; the goal being to cal- 
culate the numerical values of the three parameters 
defining the plane: !?, (p and D. Within the framework 
of the Least Squares method, the fit parameters are 
obtained by minimizing the error function 



E{i},(p,D,P,^^)- 



If £;(,?, <p, A ^o, 



(4) 



definitions of the theoretical point yield different error 
functions. In this paper we study two error functions: 
the orthogonal error function Eq and the directional 
error function is^,, as explained in Fig. 1 and in the next 
two sections. 

It is not surprising that due to nonlinear dependence 
of the normal vector w on both angles i} and (p, plane 
fitting requires nonlinear minimization. However, as is 
shown in the next two sections, for both error functions 
Eq and is^, the distance £", depends linearly on the third 
parameter D . Therefore, D may be explicitly expressed 
as a function of both angles (;?, (p) and P^,^ from the 
condition 



dD 



iA(p,D,P,^^.)=0. 



(5) 



For any error function E defined by Eq. (4) with £} 
depending linearly on D, the linear parameter can be 
expressed as a function of the remaining non-hnear 
parameters and dataset P^;^ 



where £", is the distance between the experimental point 
Pj and its corresponding "theoretical point." Different 



D^D{i},(p,P^^^). 



(6) 




Fig. 1. Due to instmment error in measurement of range r, experimental points P do not lie exactly on a plane (thick line). 
Points O are perpendicular projections of experimental points on a plane. Points D are intersections with the plane of rays 
originating from the instrument and passing through the experimental points. Distances PO are used in the orthogonal error 
function Eq while PD are used in the directional error function Ejj. The difference between both functions depends on the 
Angle of Incidence (AOI). 



462 



Volume 115, Number 6, November-December 2010 

Journal of Research of the National Institute of Standards and Technology 



When the error function E reaches its minimum 
at {-§', (p*, £>'], a gradient of the function has to be zero. 
This implies that the original 3D search space may be 
reduced to a 2D space and the error function may be 
re-written as 



E{A(P,P>^^,) = -Y^E]{A(P,P,), 



N 



(7) 



j=i 



the minimum of E being located at 



(8) 

The variances of the fitted plane parameters var(t?*) 
and var((p*) may be calculated following the same 
general approach developed for fitting a sphere to range 
data [8], [9]. In the current study, the same assumption 
is made as in the previous studies: for the 3D imaging 
systems relevant to this study, the uncertainty in the 
range measurement is typically much larger than the 
uncertainty in the angular measurements. Thus, an 
acquired point Pj can be expressed as 



)Pj{^j,(Pj), 



(9a) 



where r^ is a range measured at bearings (t?,, (Pj) and 
\\Pj\\ = Ty. In this approximation the bearings are treated 
as noise-free control variables and a unit vector pj is 
defined as 

Pj {t}j , (Pj ) = [cos t}j cos (Pj , COS t}j sin cpj , sin t}.] . (9b) 



Note that for other types of instruments, for example 
Coordinate Measuring Machines (CMM), the above 
assumption may not be valid and the formulas for 
variances of fitted plane parameters developed in this 
paper may not be applicable. In addition, it is assumed 
that the correlation in the measured ranges r^ and r^ is 
neghgible for anyjj^k. When both assumptions are 
valid, the variances of the fitted plane parameters may 
be estimated by applying to Eq. (8) the uncertainty 
propagation formula [2] 



van 



(t?*) » s 



/=1 



dr. 



var(r^.) , 



(10a) 



van 



(<P*)»I 



7=1 



dr. 



var(r.) , 



(10b) 



and the covariance may be estimated as 



C0V(1?*,()5*)»^ 



j=i 



[ dr^ \ 


[ dr^ \ 



var(r,,). (10c) 



The variance of the third parameter D may be 
calculated from the uncertainty propagation formula 
[2] applied to a general function D{-d,(p,P^fj^) defined 
in Eq. (6), 



r(^*) = I 



7=1 



do a??* do d(p' do 

di} dr. d(p dr. dr 



r{rj) (11) 



where the derivatives of D are calculated at 

iTf,(p\Pm)- 

The individual sensitivities ^r — , ^r — used in Eqs. (10) 

dr. dr. 

and (11) may be calculated as in [8] by solving for each 
j the following 2x2 system of linear equations 



//(7>*,9*,^,,,,) Sj(P^,,) = -V.(i^\(p\P,^,.), 



where the vectors Sj and F/ are defined as 



(12) 



SAP.,.0- 



dT}'iP..m) 



dr 

d(p\P,,.) 
dr, 



(13a) 



F.(7?,(p,i^,„,) = 



d' E{T},(p,P,^^^) 

dr^di} 

d'Eii},(p,P,^^^^) 

dr.d(p 



(13b) 
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The matrix H is the Hessian of the error function 



H(P,(P,P,^,) 



d'E d'E 



(14) 



where 



Eo(^,9,P:,..,) = -t.(^''Uj)\ (19a) 



N% 



u, = P,-P, 



(19b) 



These general formulas are now applied to two 
specific error functions: the orthogonal error function 
Eq and the directional error function is^. 



Then, the elements of the gradient of the error func- 
tion VEq defined in Eq. (19a) can be calculated as 



3. Orthogonal Fitting 






(20a) 



For the orthogonal plane fitting (see Fig. 1) the 
theoretical point Oj corresponding to the experimental 
point Pj is defined as the orthogonal projection of 7*, on 
a plane. Thus, Eq. (4) takes the form 



dE, 
dip 



2 ^ 



{^,^,P,,,) = -Y,{w-U^) 



N 



j=i 



dw 
d(p 



•U: 



(20b) 



E„{i»,(p,D,P,^,,^) = -Y^[w{i»,(p)^ P. - Df . (15) 



N 



y=i 



while the elements of the Hessian matrix H defined in 
Eq. (14) are 



Applying condition (5), Equation (6) can be expressed 

as 



D{p,(p,P,,,;) = w{^,(py 



r -^ N \ 



:I^ 






wP,, (16) 



d'E 



d^ 



H^,<P,P>.m) 



T N 
-I 



dw 



•U: 



\^'U^) 



/32 A 



where Pq here is the centroid of all experimental points 
Pj^ . This condition states that the plane fitted with the 
orthogonal error function has to contain the centroid, 
Pq. Defining a scalar product d^ of two vectors w and Pj 
given by Eqs. (2) and (9b) 



then Eq. (16) for the parameter/) when using Eqs. (17) 
and (9a) can be rewritten as 



(21a) 



d'E. 



dip 



fi^'(P,Pm) 



T N 



d(p ' 



{w-U^) 



'd' 



a/-^^ 



(21b) 



1 ^ 



D{^,(p,P^,.^) = -J^d^(i3,(p)rj, 



^M 



(18) 



where r^ is a measured range. In this notation, Eq. (7) 
describing the error function in the reduced 2D search 
space of angles (t?, <p) can be written as 



di^dcp 



iP,<P,Pm) 



iV 



at? 



.[/ 



va, 



dip 



u, <»"U^) 



^ d'w 



\ 



.[/ 



di}d(p ' 



/J 
(21c) 
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Finally, the elements of vector V^ defined in 
Eq. (13b) can be obtained by differentiating with 
respect to r^ the elements of the gradient VEq given by 
Eqs. (20a) and (20b). Taking into account the definition 
of vector Uf given by Eq. (19b) and the definition of 
centroid Pq of all points P/ as well the dependence of 
Pj on Ty given by Eq. (9a), the elements of vector F, can 
be evaluated as 



^(^,<P,P,,,,)- 
dr. drt 






dw 



•c/. 



iv+(w«C/j) 



dw 



dr.d(p 






i^,(p,P..m) 



J LV 



N] 



dip 



•u, 



iv+(iv«C/j) 



aw 

d(p 



where 



(22a) 



(22b) 



4, Directional Fitting 

For the directional plane fitting (see Fig. 1) the 
theoretical point Dj corresponding to the experimental 
point Pj is defined as an intersection of a ray originat- 
ing from the instrument through Pj with the plane 



D,=hP, 



(24a) 



where a parameter tj has its value close to 1 , and the 
theoretical points satisfy Eq. (1) of the plane 



D^{x^,y^,z.)»w{T^,(p)=D. 



(24b) 



The distance is, in Eq. (4) is the Euclidian norm and 
the directional error function Ed can thus be written as 



e^{P,(P,d,p,^.) = -Y}d^-p\ 



N% 



(25) 



ac^ 

dr, 



■^J.k 



N 



Pt' 



(22c) 



5/ 1, is the Kronecker delta, and Pi, is the unit vector 
defined in Eq. (9b). First and second order derivates of 
the vector w{t},(P), defined in Eq. (2), are provided in 
Appendix A, Eqs. (A1-A5). Once the matrix H and 
vectors F, are known, the sensitivity vectors 5, can be 
calculated for every j by solving the 2x2 system of 
linear Eq. (12). The variances of the fitted angles 
var(??*) and var((p*) and the covariance cov(??*,(p*) can 
then be determined from Eqs. (10a, b, c). The variance 
of the third parameter D*, defined in Eqs. (16-18), can 
be now evaluated from Eq. (11) using the following 
equations: 



do _ dw 

a^~a^' 



• p. 



(23a) 



where the parameter t^ can be calculated from Eq. (24) 
using the dj defined in Eq. (17) 



D 

r. d^ 



(26) 



if dj is different from zero, i.e., if the vector pj is not 
orthogonal to w. Two vectors Pj and w are orthogonal 
only if the corresponding AOI = ± 90°, which causes 
the theoretical point Z)y to be undefined. For all other 
AOIs, tj can be calculated and substituted into 
Eq. (24a). Then, using Eq. (9a) and the fact that 
r,= II Pj II, Eq. (25) yields the following expression for 
the directional error function 



£„(7?,(p,A^,„,)=^i; 



N^, 



r. 



(27) 



dD _dw 
dip dip 



•n> 



(23b) 



Applying condition (5) to Eq. (27), Eq. (6) can be 
expressed as 



dD _ 1 

ar, ~ N 



df , 



(23c) 



where the derivatives of D are calculated at 

[r,(p*,P,^]. 



D{i},(p,P,,^)-- 



lr,d-^ 



j=i 



7 = 1 



(28) 
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In this notation, Eq. (7) describing the error function 
in the reduced 2D search space of angles ('&, (p) can be 
written as 






AA.,+ 



D 

'', 



dA. 



OJ 



d(p 
(32c) 



^ 7=1 



D(^,(P,P,,^,) 



dj{i},(p) 
Using the following auxiliary functions 






r 

d 



V ' J 



d'-^-o'-^ 
' at? at? 



V 



(29) The derivatives — and — in Eqs. (30a,b) are calcu- 

at? d(p 

dd ■ ddi 

lated in the Appendix B, Eqs. (B 17, B 18), — ^and — - 

at? d(p 

in (A6, A7), — ^, — ^ and -^ in (C1-C3). 
di^ d(p dip 

(30a) Finally, the elements of vector F defined in Eq. (13b) 

can be expressed as 



D 



d_ 
dip 



d. ' 

V ' I 



d. D- 

' d(p d(p 



d/ , 



(30b) 



it is possible to calculate the elements of the gradient of 
the error function Vis^, 



'^f(^''^'^-)4i 



'd ^ 



at? 



N 



7=1 



Aj , (31a) 



V ' J 



d'E 



dr. at? 

Ay 



5:(^''P'^!«>> 



do 1 


\ 




/ 


dr. 


-<^M 


\k + 






/ 




V 



D 



aA 



dr 



(33a) 



a^£ 



dr d(p 

Ay 

Ntt 



^(t?,.p,7>„,)= 



do 

— I 
dr. 



'/.* 



<k + 



D 



dA 



dr. 



^("'•^•'^••'^^I 



D 



A (31b) 



and the elements of the Hessian matrix H defined in 
Eq. (14) 



^>(t?,^,p,„,)4i 



at?' 



A^- 



aiE, 

d(p 



f(^''P'nv,)=^I 



N 



7=1 



<7 + 



At ,. + 



r. 



r. 

V ^7 ' , 



dA 



fi.j 



at? 



(32a) 



a A 



d(p 



(32b) 



(33b) 



az) 



where the derivative — is calculated in Appendix B, 
dr. 

Eq. (B22), — ^and ^^in Eqs. (C4, C5), and 5.^is 
dr. dr. 

the Kronecker delta. Once the matrix H and vectors Vj 
are known, the sensitivity vectors Sj can be calculated 
for every j by solving a 2 x 2 system of linear equa- 
tions (12). The variances of fitted angles var(t? ) and 
var((p ) and the co variance cov(t? ,(p ) can then be 
determined from Eqs. (10a, b, c). The variance of 
the third parameter D , defined in Eq. (28), can now 
be evaluated by substituting in Eq. (11) the Eqs. (B17, 

B18, B22), for the derivatives — , — and — 

at? d(p drj 

calculated at [t?',(p', Pj^j] . 
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5, Discussion 

Figure 1 shows that the orthogonal plane fitting 
behaves differently from the directional fitting. The 
difference between orthogonal and directional fitting 
depends on the Angle of Incidence (AOI) of the laser 
beam. For AOI approaching 90°, the optimal value of 
the orthogonal error function E q{-&* , cp* , P ^pj^^) is 
decreasing, even when the uncertainty of the measured 
ranges r^ is large. The optimal value of the error func- 
tion is usually interpreted as a gauge of noise level in 
experimental data (assuming that a right model is fitted 
to the data). For 3D imaging systems, due to a diver- 
gence of a laser beam, range measurements collected 
for large AOI have large uncertainty. Thus, the behav- 
ior o^Eq is in a sharp contrast with the common exper- 
imental observation. The directional fitting is free of 
this flaw and a residual value of the directional error 
function E[,{-d*, cp*, P^^j,^ ) correctly estimates a level of 
noise in the acquired experimental dataset Pj^ for any 
AOI. When Eq is minimized, the sensitivities of the 

fitted angles and may be underestimated for 

dr. dr 

large AOI. The flawed sensitivities entered in the 
Eqs. (10) and (11) will cause an underestimation of the 
variances of the plane parameters fitted with the 
orthogonal function for large AOI. For small AOI, the 
difference between Eq and 5^ is diminishing and 
both error functions are expected to provide correct 
estimates for the variances of fitted parameters. 

Individual sensitivities 5, of fitted angles are calcu- 
lated from Eq. (12). The vector F, on the right hand side 
of this equation behaves differently for the orthogonal 
and the directional error function, see Eqs. (Dl, 2) and 
(D7, 8) in the Appendix D. This may cause a much 
larger variability of S/ calculated for the orthogonal 
fitting and a poorer estimate of variances of fitted 
parameters than for the directional fitting. 

As was already pointed out, a plane fitted by mini- 
mizing the orthogonal error function has to contain the 
centroid Pq of the acquired points P^fp^, see Eq. (16). 
Directional fitting does not have this constraint. Thus, a 
minimization of two error functions discussed in this 
paper may lead to different results. 



6. Conclusions 



systems. Two error functions were investigated: the 
orthogonal and the directional error function. 
Comparison of corresponding formulas suggests the 
two functions may yield different results when applied 
to the same range data. However, in order to quantify 
the anticipated difference, laboratory experiments and 
computer simulations are needed. 

7, Appendix A 

From the definition of w (??, (p) in Eq. (2), the follow- 
ing derivates can be calculated 



dw 



dw 
dip 



[-sini?cos()f), -sini?sin ()f), COS??] (Al) 



[- cos T& sin (p, cos i?cos 9, ] (A2) 



d'w 



d'w 
dcp' 



(A3) 



[- cos ?? COS (p, - COS ??sin (jo, ] (A4) 



d'w 
dT}d(p 



[sin i?sin (p, -sin j?cos (p, ]. (A5) 



Then, the corresponding derivatives of d/{'&,(p) 
defined in Eq. (17) can be expressed as 



ddj dw 



3 dj dw 
d(p d(p 



•Pj 



'Pj 



av, 


d'w 


di}' 


dl}' 


d% _ 


d W 



d(p' dip' 



'PJ 



'PJ 



(A6) 



(A7) 



(A8) 



(A9) 



In this paper we derived formulas for the variances 
(which are useful for uncertainty analysis) of plane 
parameters fitted to a dataset acquired with 3D imaging 



av, 



a^ 



d(p d-d d(pd-d 



'Pj 



(AlO) 
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8, Appendix B 

Auxiliary functions defined for calculation of deriva- 
tives ofZ)(t?,(p, PjA,}) defined in Eq. (28) for the direc- 
tional fitting are given by: 



a(7},(p,P^^^)=f^d:' 
bi^,(p,P.,.i)=f,rjd:' 

7=1 

d^{^,cp,P^,.^)=j^r^^df 



;=i 



dd- 



^.(^''P'^(A'!)=io^< 



(Bl) 
(B2) 
(B3) 
(B4) 
(B5) 
(B6) 



3c„ 
dip 


N 
7=1 


dddip ' 


dd. dd. 

3d-' ' ' 

' dip 3i? 


dd, 
dip 


N 

= 1 

7=1 


dddip ' 


dd. dd.' 
2d-' ' ' 
' dip dd 



(B15) 



r. (B16) 



where the derivatives of d^ are defined in (A6-A10). 
Using the above functions, the derivatives of 
D{d, ip, P{N]) can be expressed as 



3D 

dd 

dD 

dip 



d'D 

dd' 



{2bc, -ad,)a ' 
(2bc^-ad^)a-' 
3c,, da 



(B17) 
(B18) 



.36 

2 — c,+2b 
dd " 



dd dd 



dd, 

fl, -a 

dd 



7=1 



■ 2a ' (2 be. -a d, ) 

dd^ " "^ 



(B19) 



Their respective derivatives are: 
da 



dd 

da 
dip 

db_ 
dd~ 

db__ 
dip 



dd j^, 



dc 'V 

—i ^ = y 

dip P 



-2 c., 



-2 c,. 



3^, , 

— ^d-' -3d:' 

dd' ' 



'ddj ' 

~dd 
V / 



d'd. , .{dd ^' 

'df -3d 



dd, 
dd 


N 

= 1 

7=1 


dip 


.V 

= 1 

7=1 



dip' ' 
d'd 



dd 
d'd 



^d:' -2d:' 

2 7 7 



-d:' -2d: 



dip 






dip 



(B7) 
(B8) 
(B9) 

(BIO) 
(Bll) 

(B12) 
(B13) 
(B14) 



d'D 

dip' 



2 — c +2o — -- — d -a — — 
dip dip dip dip 



— 2a ' (2bc„ —ad ,) 

dip^ " "^ 



(B20) 



d'D 
dipdd 



db 



3c,, do 



2 — c,+2b—!^ 
dip dip dip 



d, — a- 



dd, 
dip 



- 2a — (2 be, —a d, ) . 
dip 



(B21) 



For the calculation of variances of fitted plane para- 
meters, the following derivatives of D{d,ip,P^fj,^) are 
also needed 



dD 



dr. dj(d,ip) a(d,ip) 



d'D 
dr.dd 



d'D 
drdip 



dd da 

— -a + d . — 
dd ' dd 

j2 2 

a,, a 



dd. da 

— -a + d - — 
dip ^ dip 

j2 2 

d . a 

7 



(B22) 



(B23) 



(B24) 
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9, Appendix C 



10, Appendix D 



The derivatives of the functions ^,,, and A^j defined 
in Eqs. (30a,b) and used in the gradient and Hessian 
calculations can be expressed as 



di} 



A,,(^,'P,^,«,)= 



2j \ 



,1^-d''^ 



'dj}' 



d1}' 



-2d^.—^^— + 2d,D 



' at? at? 



dd. 



(CI) 



dip 



Aii^'^'^m)'- 



',tR-/''^'' 



d(p' d(p' 



, dd. do 



^dd ^ 



' d(p d(p 



dm 



(C2) 



d(p 



AjiT^,(p,PiNi)-- 



ddj do d^D _ do dd. 

d(p at? ' dcpdi} d(p at? 



d d dd, 

-D '--2d.—^A, 

dcpdi} ' dcp "- 



(C3) 



Using Eq. (22c), Eqs. (22a) and (22b) can be rewrit- 
ten in the following form 



drj at? 



d'E^ 



(Dl) 



^(t?,(p,7>„,)=-[i^,.,.(t?,(p,7>„,)-/V(t?,()r), 7>„,)], 

or. o(p Jy 

where 

F,j(.^,(p,Pw) = Pj 



LV 



at? ' 



1^+ 



(wu^y 



(D2) 
dw 






W + 



(h..C/^) 



at? 

(D3) 

aw 

d(p 
(D4) 



and F^, F^ are the averaged values 

F,M(p,P,,.,) = ^f,F,M(P,Pi.,) 



(D5) 



F/i>,(p,P.,^) = -X^,,.(^' (P, ^«,) • (D6) 



where the derivatives of Z)(t?, (p, P{tfi) and d^ are calcu- 
lated in Appendices A and B. For the evaluation of 
sensitivities of fitted plane parameters, the following 
derivatives are also required 



Equations (Dl) and (D2) show that the components 
of the vector F, in Eq. (13b) applied to Eq depend 
explicitly on the measured range r^. 

Equations (33a) and (33b) can be rewritten in the 
following form 



dr 



dA, 



(p,k 



dr. 



\ d^_dDdd^^ 

' ar.at? dr at? 



^ , d^D do dd, ^ 

dk 

drdcp dr. dcp 



(C4) 



(C5) 



where the derivatives of D are given by Eqs. (B22- 

B24). 



B^'"*"'-'- 



N 



do , 

A},k -, "t + 

dr. 



^D .V4, 



V « y 



dr 



\, 



1^^^'^'^-^^ 






\.k^d, + 
dr, 



' D ^ 



V ' J 



dA 



(p,k 



dr 



(D7) 



(D8) 
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From the Eqs. (C4), (C5), and (B22) it follows that 

none oi the derivatives — — , — — , or -— depends 
ar dr. dr. 

on Ty . In fact, a measured range r^ enters the right hand 
side of Eqs. (D7) and (D8) only indirectly in the formu- 
la for £) and its angle derivatives; see Eqs. (28), (30a,b), 
(B17) and (B18). Thus, the influence of a particular 
y'-th range measurement on the components of the 
vector Vf in Eq. (13b) applied to E^, is neghgible for 
typical datasets with large N. 
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